#include <math.h>
#include <iostream>
#include "MorphologyFeatExtractor.h"
using namespace std;
/*******************************************************************/
CMorphologyFeatExtractor::CMorphologyFeatExtractor(void)
{
	m_sz = cvRect(0,0,0,0);
	m_contourLength=0.00;
	m_contourArea=0.00;
	m_aspectRatio=0.00;
	m_memStorage = cvCreateMemStorage();
	m_contour = NULL;
	m_LineHist = NULL;
	m_CircleHist = NULL;
	m_sampleArea = 0;
	m_samplePerimeter = 0;
	m_orientationBins = 6;
	m_lengthBins = 3;
	m_threshold = 150; 
}
/*******************************************************************/
CMorphologyFeatExtractor::~CMorphologyFeatExtractor(void)
{
	cvClearMemStorage(m_memStorage);
	cvReleaseHist(&m_LineHist);
	cvReleaseHist(&m_CircleHist);
}
/*******************************************************************/
TFeatureVector CMorphologyFeatExtractor::ExtractFeaturesFromImage( IplImage * img )
{
	TFeatureVector retVal;
	if( img->nChannels > 1 ) 
	{
		IplImage * bwImg = ImgToBW(img);
		ExtractContourStats(bwImg);
		ExtractLineHistogram(bwImg); 
		cvReleaseImage(&bwImg);
	}
	else
	{
		ExtractContourStats(img);
		ExtractLineHistogram(img); 
	}
	retVal.push_back(m_aspectRatio);
	retVal.push_back(m_contourArea); 
	retVal.push_back(m_contourLength);
	retVal.push_back(m_huMoments.hu1);
	retVal.push_back(m_huMoments.hu2);
	retVal.push_back(m_huMoments.hu3);
	retVal.push_back(m_huMoments.hu4);
	retVal.push_back(m_huMoments.hu5);
	retVal.push_back(m_huMoments.hu6);
	retVal.push_back(m_huMoments.hu7);
	if( NULL != m_LineHist )
	{
		for( int i=0; i < m_orientationBins; i++ )
		{
			for( int j=0; j < m_lengthBins; j++ )
			{
				retVal.push_back( cvQueryHistValue_2D(m_LineHist,i,j) );
			}
		}
	}
	return retVal;
}
/*******************************************************************/
TDataSet CMorphologyFeatExtractor::ExtractFeaturesFromData( CDataPointManager * dpMgr, bool loadImages)
{
	TDataSet retVal;
	if( NULL == dpMgr )
		return retVal;
	int sz = dpMgr->GetSize();
	for( int i = 0; i < sz; i++ )
	{
		if( loadImages )
		{
			dpMgr->GetDataPoint(i)->LoadSourceImage();
		}
		if( dpMgr->GetDataPoint(i)->HasImage() )
		{
			cout << "Loaded image " << i << " name: " << dpMgr->GetDataPoint(i)->GetInFile() << endl; 
			TFeatureVector temp = ExtractFeaturesFromImage( dpMgr->GetDataPoint(i)->GetImage() );
			cout << ">>>Loaded " << temp.size() << " from image " << i << endl; 
			retVal.push_back(temp);
			if( loadImages )
			{
				dpMgr->GetDataPoint(i)->FreeImage(); 
			}
		}
		else
		{
			cout << "Failed to load image: " << dpMgr->GetDataPoint(i)->GetInFile();
		}
	}
	return retVal; 
}
/*******************************************************************/
TFeatureVectorLabel CMorphologyFeatExtractor::GetLabels( void )
{
	TFeatureVectorLabel retVal;
	return retVal; 
}
/*******************************************************************/
IplImage * CMorphologyFeatExtractor::ImgToBW(IplImage* img)
{
	IplImage * retVal = cvCreateImage(cvSize(img->width,img->height),img->depth, 1);
	cvCvtColor(img,retVal, CV_BGR2GRAY);
	return retVal; 
}
/*******************************************************************/
void CMorphologyFeatExtractor::Reset(void)
{
}
/*******************************************************************/
CvSeq* CMorphologyFeatExtractor::ExtractContourStats(IplImage * img)
{
	IplImage* temp = cvCreateImage(cvSize(img->width,img->height),img->depth,1);
	IplImage* temp2 = cvCreateImage(cvSize(img->width,img->height),img->depth,1);
	// need thresh
	cvCopy(img,temp);
	cvThreshold(temp,temp2,m_threshold,255, CV_THRESH_BINARY_INV);
	cvDilate(temp2,temp2,NULL,3);
	CvSeq* contour = NULL;
	CvSeq* best = NULL;
	double length=0.00;
	double best_l = 0.00;
	CvContourScanner scanner = cvStartFindContours( temp2,m_memStorage );//, sizeof(CvContour), CV_RETR_CCOMP);
	contour = cvFindNextContour( scanner );
	vector<CvSeq*> ctrs;

	while(contour)
	{
		length = cvContourPerimeter(contour);
		if( length > best_l )
		{
			best_l = length;
			best = contour;
		}
		contour = cvFindNextContour( scanner );
	}
	
	m_contour = best;
	cvReleaseImage(&temp);
	cvReleaseImage(&temp2);
	if( m_contour )
	{
		m_sz = cvBoundingRect(m_contour);
		m_sampleArea =  m_sz.width*m_sz.height;
		m_samplePerimeter = (2*m_sz.width)+(2*m_sz.height);
		m_aspectRatio = (double)m_sz.width/(double)m_sz.height;
		m_contourLength = (double)cvArcLength(m_contour);
		m_contourLength = m_contourLength/(double)m_samplePerimeter;
		m_contourArea = (double)cvContourArea(m_contour);
		m_contourArea = m_contourLength/((double)m_sampleArea);
		CvMoments moments;
		cvContourMoments(m_contour,&moments);
		cvGetHuMoments(&moments, &m_huMoments);
	}
	return contour;
}
/*******************************************************************/
IplImage * CMorphologyFeatExtractor::GetEdgeImage(IplImage* img)
{
	IplImage* temp = cvCreateImage(cvSize(img->width,img->height),IPL_DEPTH_8U,1);
	cvCanny(img,temp,50,150);
	return temp;
}
/*******************************************************************/
void CMorphologyFeatExtractor::ExtractLineHistogram(IplImage* img)
{
	IplImage * lineImg = GetEdgeImage(img); 
	char * fname=NULL;
	fname = new char[48];
	//sprintf(fname,"DataPoint%iEdge.png",m_id);
	//cvSaveImage(fname,lineImg);
	double rho = 1; // don't discritize 
	double theta = CV_PI/180.0; // 2 degree increments 
	int thresh = 7; // minimum number of points on the line 
	double min_length = 3; // minimum length
	double min_space = 3; //distance btw colinear segments before separation 
	CvSeq* lhist = cvHoughLines2(lineImg,m_memStorage,CV_HOUGH_PROBABILISTIC, rho, theta, thresh, min_length, min_space); 
	if( lhist->total > 0 )
	{
	//printf( "Found %i points\n",lhist->total);
	// 2 dimensions, 10 degree increments and an equal number of spatial bins
	CvArr** histData = new CvArr*[2];
	histData[0]= cvCreateMat(lhist->total,1, CV_8U);// <-- CV_8U IS TOTAL BULLSHIT
	histData[1]= cvCreateMat(lhist->total,1, CV_8U);// <-- BUT IT WON'T WORK OTHERWISE
	
	for( int i=0; i < lhist->total; i++)
	{
		CvPoint* points = (CvPoint*)cvGetSeqElem(lhist,i);
		double x = abs((double)(points[0].x-points[1].x));
		double y = abs((double)(points[0].y-points[1].y));
		double l = (sqrt((x*x+y*y))/((double)m_samplePerimeter));
		double a = (double)atan2(y,x);
		a = 360.00f*(a/(2*CV_PI));
		if( a < 0 )
		{
			a = 360.00f+a;
		}
		cvSet1D(histData[0],i,cvScalar(a));
		cvSet1D(histData[1],i,cvScalar(l));

	}
	int bins[] = {m_orientationBins,m_lengthBins};
	m_LineHist = cvCreateHist(2,bins,CV_HIST_ARRAY);
	if( lhist->total > 0 )
	{
		cvCalcArrHist(histData,m_LineHist);
		cvNormalizeHist(m_LineHist,1.00);
	}
	for(int i=0; i<2;i++)
	{
		CvArr* temp = histData[i];
		cvRelease(&temp);
	}
	if( NULL != histData )
	{
		delete [] histData; 
	}
	}

	cvReleaseImage(&lineImg);
}
/*******************************************************************/